#include "lane_feature_extraction_v0.h"
#include "calculate_tools.h"

//#define _DEBUG_SAVE_SHOW_2FEATURE_LANE

namespace rmd {
LaneFeatureExtraction::LaneFeatureExtraction() { GetThresholdValue(); }

LaneFeatureExtraction::~LaneFeatureExtraction() {}

void LaneFeatureExtraction::SetDataFrameToClass(DataFrame* data_frame,
                                                cv::Mat& pm_black_out) {
  data_frame_ = data_frame;
  this->pm_gray_image = data_frame->process_grayimage;
  this->pm_black_out_ = pm_black_out;

  this->far_line_ = data_frame->far_line;
  this->middle_start_line = data_frame->middle_start_line;
  this->start_line = data_frame->start_line;
  this->end_line_ = data_frame->end_line;
  this->line_list_ = data_frame->line_list;

  this->detect_line = data_frame->pri_refine_results;

  boundary_list_[0] = data_frame->up_boundary;
  boundary_list_[1] = data_frame->left_boundary;
  boundary_list_[4] = data_frame->right_boundary;
  boundary_list_[2] = cv::Point(boundary_list_[1].x, end_line_);
  boundary_list_[3] = cv::Point(boundary_list_[4].x, end_line_);

  lane_width_limit = data_frame->lane_width_limit;

#ifdef _DEBUG_SAVE_SHOW_2FEATURE_LANE
  data_frame->process_image.copyTo(lane_feature_show);
#endif
}

void LaneFeatureExtraction::Update(DataFrame* data_frame,
                                   cv::Mat& pm_black_out) {
  SetDataFrameToClass(data_frame, pm_black_out);

  int nLine = line_list_.size();

#ifdef _DEBUG_SAVE_SHOW_2FEATURE_LANE
  const cv::Point* pt[1] = {boundary_list_};
  int npt[] = {5};
  cv::polylines(lane_feature_show, pt, npt, 1, 1, cv::Scalar(0, 0, 0), 2);
#endif

  uchar* gray_image = pm_gray_image.data;
  int nCols = pm_gray_image.cols;

  for (int iLine = 0; iLine < nLine; ++iLine) {
    int iRow = line_list_[iLine];
    short iLeftBound = boundary_list_[1].x;
    short iRightBound = boundary_list_[4].x;

    if (iRow > boundary_list_[0].y) {
      if (iRow < boundary_list_[1].y) {
        cv::Point ptempLeft =
            CalcuateTools::GetInstance()->GetPointByDoublePoint(
                boundary_list_[0], boundary_list_[1], iRow, 1);
        iLeftBound = max((short)ptempLeft.x, iLeftBound);
        cv::Point ptempRight =
            CalcuateTools::GetInstance()->GetPointByDoublePoint(
                boundary_list_[0], boundary_list_[4], iRow, 1);
        iRightBound = min((short)ptempRight.x, iRightBound);
      } else {
        iLeftBound = rmd::max(iLeftBound, kRoiOffset);
        iRightBound = rmd::min(iRightBound, (short)(kImageWidth - kRoiOffset));
      }
    } else {
      break;
    }

    std::vector<uchar> grayValue;
    std::vector<UdPair> dPair;
    int iIndex = iRow * nCols;
    for (int j = iLeftBound; j < iRightBound; ++j) {
      grayValue.push_back(gray_image[iIndex + j]);
    }
    DetectUpdownLane(grayValue, dPair, iRow, iLeftBound);

    if (iLine < nLine - 1) {
      int drow = iRow;
      int urow = line_list_[iLine + 1];

      if (drow - urow > 1) {
        DetectUdROILane(dPair, drow, urow);
      }
    }
  }

#ifdef _DEBUG_SAVE_SHOW_2FEATURE_LANE
  cv::imshow("Lane_Feature_Show", lane_feature_show);
//  cv::waitKey(0);
#endif
}

void LaneFeatureExtraction::GetThresholdValue() {
  int i_max = 200;
  int i_min = 50;

  float factor_min = (float)(1 / kPmFeatureInputLow);
  for (int i = 0; i < i_min; ++i) {  // 12 , 10 , 12
    threshold_value[i] = i * factor_min;
  }

  float factor_middle = (float)(1 / kPmFeatureInputMiddle);
  for (int i = i_min; i <= i_max; ++i) {  // 10 , 12 , 6
    threshold_value[i] = i * factor_middle;
  }

  float factor_max = (float)(1 / kPmFeatureInputHigh);
  for (int i = i_max + 1; i < kMaxChar; ++i) {  // 6,    6    10
    threshold_value[i] = i * factor_max;
  }
}

void LaneFeatureExtraction::DetectUpdownLane(std::vector<uchar>& colors,
                                             std::vector<UdPair>& ud_pair,
                                             int current_r, int start_c) {
  int num = colors.size();
  std::vector<bool> up_point(num, false);
  std::vector<bool> down_point(num, false);
  FindMaxUpDown(colors, up_point, down_point);
  FindLaneUDpair(up_point, down_point, colors, ud_pair, current_r, start_c);

  uchar* pImage = pm_black_out_.data;
  std::vector<cv::Point> udstudVector;
  cv::Point poi;
  int length = ud_pair.size();
  for (int i = 0; i < length; ++i) {
    poi = cv::Point((ud_pair[i].up_point.x + ud_pair[i].down_point.x) / 2,
                    (ud_pair[i].up_point.y + ud_pair[i].down_point.y) / 2);
    pImage[poi.y * pm_black_out_.cols + poi.x] = 255;
    udstudVector.push_back(poi);
  }
// SetPicPointWhite(mPMBlackOut, udstudVector);

#ifdef _DEBUG_SAVE_SHOW_2FEATURE_LANE
  CalcuateTools::GetInstance()->ShowPointsInMat(lane_feature_show, udstudVector,
                                                cv::Scalar(0, 0, 255));
#endif
}

void LaneFeatureExtraction::FindMaxUpDown(std::vector<uchar>& colors,
                                          std::vector<bool>& up_point,
                                          std::vector<bool>& down_point) {
  int num = colors.size();
  float max_up_d = 0.0, max_down_d = 0.0;
  int max_up_i = 0, max_down_i = 0;
  std::vector<bool> wave_up(num, false);
  std::vector<bool> wave_down(num, false);
  int step = 5;

  float temp_avg = 0;
  for (int i = 0; i < num; ++i) {
    temp_avg += colors[i];
  }
  if (num > 0) {
    temp_avg /= (1.0 * num);
  }

  for (int i = 1; i < num; ++i) {
    float d_value = 0.0;
    int m_value = 0;

    Neighbourvalue(colors, i, step, d_value, m_value);

    if (d_value > threshold_value[m_value]) {
      wave_up[i] = true;
      if (wave_up[i - 1]) {
        if (colors[i] - colors[i - 1] > max_up_d) {
          max_up_d = float(colors[i] - colors[i - 1]);
          max_up_i = i;
        }
      } else {
        max_up_i = i;
      }
    } else {
      if (wave_up[i - 1]) {
        if (colors[max_up_i] > temp_avg + 10) {  // 10,40
          up_point[max_up_i] = true;
        }
        max_up_i = 0;
        max_up_d = 0;
      }
    }

    if (d_value < -threshold_value[m_value]) {
      wave_down[i] = true;
      if (wave_down[i - 1]) {
        if (colors[i] - colors[i - 1] < max_down_d) {
          max_down_d = float(colors[i] - colors[i - 1]);
          max_down_i = i;
        }
      } else {
        max_down_i = i;
      }
    } else {
      if (wave_down[i - 1]) {
        down_point[max_down_i] = true;
        max_down_i = 0;
        max_down_d = 0;
      }
    }
  }
}

void LaneFeatureExtraction::Neighbourvalue(std::vector<uchar>& colors,
                                           int start, int len, float& diff,
                                           int& mean) {
  int size = colors.size();
  int sum = 0.0;
  for (int l = 0; l < len; ++l) {
    int index_start = start - l - 1;
    int index_mid = start + l;
    if (index_start >= 0 && index_mid < size) {
      sum += colors[index_mid] - colors[index_start];
      mean += colors[index_mid] + colors[index_start];
    } else {
      len = l;
      break;
    }
  }

  diff = sum / (1.0 * len);
  mean = (mean / len) >> 1;
}

void LaneFeatureExtraction::FindLaneUDpair(std::vector<bool>& up_p,
                                           std::vector<bool>& down_p,
                                           std::vector<uchar>& colors,
                                           std::vector<UdPair>& ud_pair,
                                           int current_p, int start_p) {
  UdPair point_pair;
  int len = up_p.size();
  int limit_len = lane_width_limit.size();
  for (int i = 0; i < len; ++i) {
    int min_j = i + 5;
    int max_j = i + 20;

    if (up_p[i]) {
      if (current_p > 0 && current_p < limit_len) {
        min_j = i + lane_width_limit[current_p].x;
        max_j = i + lane_width_limit[current_p].y;
      }

      if (min_j < len) {
        int size = min(max_j, len);
        for (int j = min_j; j < size; ++j) {
          if (down_p[j]) {
            {
              point_pair.up_point = cv::Point(i + start_p, current_p);
              point_pair.down_point = cv::Point(j + start_p, current_p);
              ud_pair.push_back(point_pair);
              i = j;
              break;
            }
          }
        }
      }
    }
  }
}

RoiBorder LaneFeatureExtraction::PointToBorder(cv::Point lp_pos) {
  double times_width = kPmFeatureTimesWidth;

  int col = lp_pos.x;
  int row = lp_pos.y;

  RoiBorder roi_border;
  int lane_width = (lane_width_limit[row].x + lane_width_limit[row].y) / 2;
  int temp_len = times_width * lane_width;
  roi_border.lcol = (col - temp_len) > 0 ? (col - temp_len) : 0;
  roi_border.rcol =
      (col + temp_len) < kImageWidth ? (col + temp_len) : kImageWidth;

  return roi_border;
}

void LaneFeatureExtraction::DetectUdROILane(std::vector<UdPair>& pPair,
                                            int frow, int lrow) {
  int poscol;
  RoiBorder roi_point;
  std::vector<RoiBorder> roi_list;
  int pair_size = pPair.size();

  if (pair_size > 0) {
    poscol = (pPair[0].up_point.x + pPair[0].down_point.x) / 2;
    roi_point = PointToBorder(cv::Point(poscol, frow));
    roi_list.push_back(roi_point);

    for (int i = 1; i < pair_size; ++i) {
      poscol = (pPair[i].up_point.x + pPair[i].down_point.x) / 2;
      RoiBorder troiPoint = PointToBorder(cv::Point(poscol, frow));
      if (troiPoint.lcol >= roi_point.rcol) {
        roi_list.push_back(troiPoint);
        roi_point = troiPoint;
      }
    }
  }

  std::vector<RoiBorder> roi_list_ref;
  const int detect_line_size = detect_line.size();
  for (int i = 0; i < detect_line_size; ++i) {
    cv::Point near_up_point = detect_line[i].Line_now[2];
    cv::Point near_down_point = detect_line[i].Line_now[3];
    int lanecol = CalcuateTools::GetInstance()->GetXorY(
        near_up_point, near_down_point, frow, 1);
    roi_point = PointToBorder(cv::Point(lanecol, frow));
    roi_list_ref.push_back(roi_point);
  }

  int roi_list_ref_size = roi_list_ref.size();
  if (roi_list_ref_size > 0) {
    for (int i = 0; i < roi_list.size(); ++i) {
      int lcol = roi_list[i].lcol;
      int rcol = roi_list[i].rcol;
      bool bTrue = true;
      for (int j = 0; j < roi_list_ref_size; ++j) {
        if ((roi_list_ref[j].lcol < lcol && roi_list_ref[j].rcol > lcol) ||
            (roi_list_ref[j].lcol < rcol && roi_list_ref[j].rcol > rcol)) {
          // roi_list_ref[j].rcol = rcol;
          bTrue = false;
          break;
        }
      }
      if (bTrue) {
        roi_list_ref.push_back(roi_list[i]);
      }
    }
  } else {
    roi_list_ref.assign(roi_list.begin(), roi_list.end());
  }

  roi_list_ref_size = roi_list_ref.size();
  if (roi_list_ref_size > 0) {
    uchar* gray_image = pm_gray_image.data;
    int cols = pm_gray_image.cols;

    for (int cLine = frow - 1; cLine > lrow; cLine = cLine - 2)  // cLine--
    {
      short left_col, right_col;
      for (int i_roi = 0; i_roi < roi_list_ref_size; ++i_roi) {
        std::vector<uchar> gray_value;

        if (cLine < boundary_list_[1].y) {
          cv::Point ptempLeft =
              CalcuateTools::GetInstance()->GetPointByDoublePoint(
                  boundary_list_[0], boundary_list_[1], cLine, 1);
          left_col = max(roi_list_ref[i_roi].lcol,
                         max(ptempLeft.x, boundary_list_[1].x));
          cv::Point ptempRight =
              CalcuateTools::GetInstance()->GetPointByDoublePoint(
                  boundary_list_[0], boundary_list_[4], cLine, 1);
          right_col = min(roi_list_ref[i_roi].rcol,
                          min(ptempRight.x, boundary_list_[4].x));
        } else {
          left_col = max((short)roi_list_ref[i_roi].lcol,
                         rmd::max((short)boundary_list_[1].x, kRoiOffset));
          right_col =
              min(roi_list_ref[i_roi].rcol,
                  rmd::min(boundary_list_[4].x, (int)kImageWidth - kRoiOffset));
        }

        for (int j = left_col; j < right_col; ++j) {
          gray_value.push_back(gray_image[cLine * cols + j]);
        }

        std::vector<UdPair> cPPair;
        DetectUpdownLane(gray_value, cPPair, cLine, left_col);
      }

      if (cLine > start_line) {
        cLine = cLine--;
      }
    }
  }
}
}
